NAVAL  POSTGRADUATE  SCHOOL 
Monterey,  California 


THESIS 

ARIES  NAVIGATION  SYSTEM  ACCURACY  AND  TRACK 

FOLLOWING 

by 

Thanh  V.  Nguyen 

March  2002 

Thesis  Advisor: 

Anthony  J.  Healey 

Approved  for  public  release;  distribution  is  unlimited 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


REPORT  DOCUMENTATION  PAGE 


FormApprovedOMBNo^  0704-018^^ 
Public  reporting  burden  for  this  collection  of  information  is  estimated  to  average  1  hour  per  response,  including 
the  time  for  reviewing  instruction,  searching  existing  data  sources,  gathering  and  maintaining  the  data  needed,  and 
completing  and  reviewing  the  collection  of  information.  Send  comments  regarding  this  burden  estimate  or  any 
other  aspect  of  this  collection  of  information,  including  suggestions  for  reducing  this  burden,  to  Washington 
headquarters  Services,  Directorate  for  Information  Operations  and  Reports,  1215  Jefferson  Davis  Highway,  Suite 
1204,  Arlington,  VA  22202-4302,  and  to  the  Office  of  Management  and  Budget,  Paperwork  Reduction  Project 
(0704-0188)  Washington  DC  20503. 

1.  AGENCY  USE  ONLY  (leave  Wan*;  I  2.  REPORT  DATE  I  3.  REPORT  TYPE  AND  DATES  COVERED 


March  2002  I  Master’s  Thesis 


4.  TITLE  AND  SUBTITLE:  ARIES  Navigation  System  Accuracy  and  Track 
Following 

5.  FUNDING  NUMBERS 

6.  AUTHOR(S):  Thanh  V.  Nguyen 

7.  PERFORMING  ORGANIZATION  NAME(S)  AND  ADDRESS(ES) 

Naval  Postgraduate  School 

Monterey,  CA  93943-5000 

8.  PERFORMING 

ORGANIZATION  REPORT 
NUMBER 

9.  SPONSORING  /MONITORING  AGENCY  NAME(S)  AND  ADDRESS(ES) 

N/A 

10.  SPONSORING/MONITORING 
AGENCY  REPORT  NUMBER 

11.  SUPPLEMENTARY  NOTES  The  views  expressed  in  this  thesis  are  those  of  the  author  and  do  not  reflect  the  official 
policy  or  position  of  the  Department  of  Defense  or  the  U.S.  Government. 

12a.  DISTRIBUTION  /  AVAILABILITY  STATEMENT 

Approved  for  public  release;  distribution  is  unlimited 

12b.  DISTRIBUTION  CODE 

13.  ABSTRACT  (maximum  200  words) 

One  of  the  greatest  challenges  associated  with  the  Autonomous  Underwater  Vehicle  (AUV)  is  reliability, 
accuracy,  and  the  high  precision  navigation  system  for  its  submerged  operations.  Data  collected  for  later  analysis 
can  be  meaningful  if,  and  only  if,  the  precise  location  of  the  vehicle  is  known  at  the  time  the  information  is 
recorded.  A  reliable  AUV  must  be  able  to  determine  its  global  position  in  the  absence  of  external  transmitting 
devices.  Dead  reckoning  is  unreliable  because  of  current  conditions  and  random  errors  in  the  velocity 
measurement  that  can  be  integrated  and  propagated  in  position  calculations  for  long  distance  submerged  travel. 
The  alternative  is  the  optimal  integration  of  all  available  organic  vehicle  sensors  to  determine  vehicle  position. 
This  requires  the  Kalman  filtering  method  which  merges  all  available  vehicle  sensors  to  estimate  position.  The 
AUV  ARIES  was  operated  in  the  Azores  from  August  10-12,  2001.  All  information  were  recorded  and  transferred 
into  several  files  for  all  the  mission  runs  during  the  exercise.  This  thesis  investigated  the  accuracy  of  the  Kalman 
filter  navigation  system  during  those  runs.  The  thesis  also  examines  the  actual  vehicle  tracks  during  the 
experiment  with  both  the  design  tracks  and  the  model  prediction  tracks  built  using  a  simulation  of  the  vehicle  track 
following  behavior. 

14.  SUBJECT  TERMS  Navigation,  Track  Following  15.  NUMBER  OF 

PAGES 

_ 97 _ 

16.  PRICE  CODE 

INSECURITY  18.  SECURITY  19.  SECURITY  20.  LIMITATION 

CLASSIFICATION  OF  CLASSIFICATION  OF  THIS  CLASSIFICATION  OF  OF  ABSTRACT 

REPORT  PAGE  ABSTRACT 

_ Unclassified _ Unclassified _ Unclassified _ UL _ 

NSN  7540-01-280-5500  Standard  Form  298  (Rev.  2-89) 

Prescribed  by  ANSI  Std.  239-18 


1 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


n 


Approved  for  public  release;  distribution  is  unlimited 


ARIES  NAVIGATION  SYSTEM  ACCURACY  AND  TRACK  FOLLOWING 


Thanh  V.  Nguyen 

Lieutenant  Commander,  United  States  Navy 
B.S.,  University  of  Memphis,  1989 


Submitted  in  partial  fulfdlment  of  the 
requirements  for  the  degree  of 


MASTER  OF  SCIENCE  IN  MECHANICAL  ENGINEERING 


from  the 


NAVAL  POSTGRADUATE  SCHOOL 
March  2002 


Author:  Thanh  V.  Nguyen 


Approved  by:  Anthony  J.  Healey 

Thesis  Advisor 


Terry  R.  McNelley 

Chairman,  Department  of  Mechanical  Engineering 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


IV 


ABSTRACT 


One  of  the  greatest  challenges  associated  with  the  Autonomous  Underwater 
Vehicle  (AUV)  is  reliability,  accuracy,  and  the  high  precision  navigation  system  for  its 
submerged  operations.  Data  collected  for  later  analysis  can  be  meaningful  if,  and  only  if, 
the  precise  location  of  the  vehicle  is  known  at  the  time  the  information  is  recorded.  A 
reliable  AUV  must  be  able  to  determine  its  global  position  in  the  absence  of  external 
transmitting  devices.  Dead  reckoning  is  unreliable  because  of  current  conditions  and 
random  errors  in  the  velocity  measurement  that  can  be  integrated  and  propagated  in 
position  calculations  for  long  distance  submerged  travel.  The  alternative  is  the  optimal 
integration  of  all  available  organic  vehicle  sensors  to  determine  vehicle  position.  This 
requires  the  Kalman  filtering  method  which  merges  all  available  vehicle  sensors  to 
estimate  position.  The  AUV  ARIES  was  operated  in  the  Azores  from  August  10-12, 
2001.  All  information  were  recorded  and  transferred  into  several  files  for  all  the  mission 
runs  during  the  exercise.  This  thesis  investigated  the  accuracy  of  the  Kalman  filter 
navigation  system  during  those  runs.  The  thesis  also  examines  the  actual  vehicle  tracks 
during  the  experiment  with  both  the  design  tracks  and  the  model  prediction  tracks  built 
using  a  simulation  of  the  vehicle  track  following  behavior. 
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I. 


INTRODUCTION 


A.  BACKGROUND 

Throughout  naval  history,  especially  in  the  last  fifteen  years,  when  compared  with 
modem  aircraft  carriers,  combatant  ships,  and  submarines,  mine  warfare  ships  have  been 
a  somewhat  less  glamorous  aspect  of  the  profession.  When  a  mine  stmck  the  USS 
Samuel  B.  Roberts  in  1988  and  during  the  Persian  Gulf  War,  mines  made  naval  warships 
vulnerable.  Mines  are  inexpensive  to  produce  and  they  do  not  require  a  lot  of 
sophisticated  technology.  Mine  warfare  is  effective  and  represents  the  single  largest 
threat  to  naval  forces  operating  in  shallow  water  regions.  Since  the  end  of  World  War  II, 
more  than  80  percent  of  the  U.S.  Navy  ships  damaged  by  enemy  action  were  the  victims 
of  mines.  Naval  leadership  has  overlooked  the  threat  of  mines.  As  a  result,  our 
adversaries  have  been  able  to  use  mine  warfare  to  their  advantage.  When  the  USS 
Princeton  and  USS  Tripoli  stmck  a  World  War  II  mine  during  the  Persian  Gulf  War,  it 
became  clear  that  the  mine  problem  reaches  farther  than  littoral  regions.  Mines  dismpt 
our  ability  to  project  power  at  sea  and  our  ability  to  conduct  amphibious  landings  on 
enemy  shorelines  [Ref  3]. 

Today’s  naval  forces  have  very  limited  capability  to  counter  the  mine  threat.  The 
U.S.  mine  countermeasures  force  consists  of  mine  warfare  ships.  Explosive  Ordnance 
Disposal  units,  and  helicopters  equipped  with  the  latest  airborne  mine  detection  and 
neutralization  systems.  These  solutions  are  the  first  step  toward  achieving  a  robust  mine 
warfare  capability  in  the  naval  forces.  To  graduate  from  a  limited  detection  and 
avoidance  capability  to  a  level  that  can  perform  the  full  spectmm  of  covert  functions,  the 
Navy  will  need  a  network  of  Autonomous  Underwater  Vehicles  (AUV)  [Ref.  2].  These 
will  have  sensors  and  the  capability  to  conduct  covert  mine  warfare  operations  after  being 
launched  from  their  “mother”  platforms.  This  thesis  investigated  the  navigation  system 
accuracy  of  the  AUV  ARIES  and  examines  the  actual  vehicle  tracks  during  the 
experiment  with  both  the  design  tracks  and  the  model  prediction  tracks  built  using  a 
simulation  of  the  vehicle  track  following  behavior. 
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B.  UNDERWATER  VEHICLE  NAVIGATION  CONCERNS 

The  primary  concern  for  AUV  systems  is  the  reliability,  accuracy,  and  high 
precision  navigation  system  for  its  submerged  operations  [Ref  1],  Data  collected  for  later 
analysis  can  be  meaningful  if,  and  only  if,  the  precise  position  of  the  vehicle  is  known  at 
the  time  the  information  is  recorded.  A  map  of  the  ocean  floor  infested  with  mines  is  very 
crucial  to  the  decision  making  in  the  conduct  of  our  amphibious  operations.  This  map  is 
useless  if  the  AUV  position  or  the  reference  point  is  unknown.  Dead  reckoning  is  the 
most  basic  navigation  method  for  underwater  vehicles  with  no  inertial  system.  This 
method  measures  the  distance  traveled  by  multiplying  the  measured  velocity  by  a  fixed 
interval  of  time  to  obtain  distance  traveled  from  a  known  reference  position.  Together 
with  a  heading  sensor,  vehicle  track  can  be  obtained.  For  long  distance,  this  method  is 
unreliable  because  of  current  conditions  and  random  error  in  the  velocity  and  heading 
measurements  are  integrated  and  propagated  in  position  calculations  between  DGPS 
updates.  For  the  past  decade,  the  Naval  Postgraduate  School  AUV  used  a  precision 
gyroscope  and  the  Kalman  filter  [Ref  10].  The  Kalman  filter  integrates  the  data  from 
different  sensors  such  as:  velocity,  and  heading  with  the  time  of  GPS  update.  The 
complexity  of  the  Kalman  filter  resides  in  many  different  input  states.  With  the 
information  from  all  these  states,  the  Kalman  filter  optimally  integrates  data  and 
recursively  processes  the  measurements  to  provide  the  best  estimate  of  vehicle  position. 
Although  several  sensors  are  inputs  to  the  vehicle  control  system,  the  most  important 
sensors  in  this  integration  process  are  the  velocity  sensor  and  the  sensor  that  provides  the 
heading  reference.  References  6,  and  10  provide  details  on  Kalman  filtering  and  its  use  in 
small  AUV  navigation  system. 

C.  SCOPE  OF  THESIS 

The  main  focus  of  this  thesis  is  to  analyze  the  navigation  error  resulting  from  the 
1200kHz  RD  instruments  navigator  Doppler  Velocity  Log  that  contains  a  TCM  2 
magnetic  compass.  The  thesis  will  also  evaluate  the  track  error  percentage  as  a  function 
of  distance  the  autonomous  vehicle  travels  while  submerged. 

Chapter  II  provides  an  overview  of  the  vehicle.  A  brief  physical  description 
includes  dimensions,  construction  materials,  and  major  hardware  components. 
Operational  capability  is  also  explained.  This  chapter  puts  forth  information  on  the 
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ARIES  AUV,  the  vehicle  being  developed,  tested,  and  upgraded  by  the  NFS  AUV 
research  group.  Vehicle  underwater  flight  operational  parameters  and  its  mission  are 
entailed. 

Chapter  III  explains  the  when  and  where  the  experimental  testing  was  conducted 
on  the  ARIES  AUV. 

Chapter  IV  explains  the  approach  and  methodologies  in  which  data  from  the 
Azores  were  analyzed. 

Chapter  V  documents  the  results  of  navigational  error  one  can  expect  when  the 
AUV  travels  submerged.  Prediction  of  error  between  vehicle  tracks  versus  programmed 
tracks  with  and  without  current  conditions  is  also  included.  This  chapter  also  summarizes 
the  conclusions  of  this  thesis  on  ARIES  AUV  detailed  in  Chapters  IV  and  makes 
recommendations  regarding  on  future  testing  associated  with  this  topic  on  AUV. 
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II.  VEHICLE  OVERVIEW 


A.  NPS  AUTONOMOUS  UNDERWATER  VEHICLE 

Faculty  and  students  from  the  Naval  Postgraduate  School’s  Graduate  School  of 
Engineering  and  Applied  Science  comprise  the  NPS  AUV  Research  Group  that  was 
started  in  1987  to  advance  the  development  of  the  PHOENIX  and  ARIES  autonomous 
underwater  vehicles  and  their  control  and  navigation  performance  [Ref.  3].  The  current 
testing  platform  for  the  research  group  is  the  ARIES  AUV.  This  vehicle  is  an  updated 
version  of  the  “PHOENIX”  vehicle.  ARIES  is  a  shallow  water  communications  server 
vehicle  with  a  global  positioning  system  (GPS)  and  a  doppler  aided  inertial  measurement 
unit  (IMU).  Navigational  errors  are  corrected  by  the  DGPS  when  the  underwater  vehicle 
surfaces.  This  vehicle  has  supported  research  on  various  control  system  architectures,  as 
well  as  equipment  reliability,  navigation  accuracy,  and  communications  performance. 
The  vehicle  is  shown  in  a  DGPS  pop-up  maneuver  in  the  Azores  in  Figure  1. 


Figure  1 .  The  NPS  AUV  in  a  GPS  Pop-Up  Maneuver  in  the  Azores  (August  2001). 

The  Aries  hull  was  outfitted  in  the  fall  of  1999  and  has  recently  become  fiilly 
operational  in  the  spring  of  2000.  This  vehicle  has  been  designed  to  test  and  demonstrate 
its  ability  to  perform  for  a  network  server  platform,  ocean  survey,  targets  reacquisition, 
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and  navigational  accuracy  during  AUV  Fest  in  Gul^ort,  Mississippi  in  1999.  Figure  2 
shows  the  command  and  control  system  during  Azores  operation  on  August  2001. 


Figure  2.  Aries  Command  and  Control  System  for  the  August,  2001  Exercise. 

B.  VEHICLE  DESCRIPTION 

The  major  hardware  components  of  the  ARIES  are  shown  in  Figure  3  and  are 
described  below.  The  vehicle  weighs  225  kg  and  measures  0.4  m  wide,  0.25  m  high,  and 
approximately  3m  long.  The  hull  is  constructed  of  0.25  inch  thick  6061  aluminum.  It 
forms  the  main  pressure  hull  that  houses  all  electronic  components,  sensors,  computers, 
and  six  12  volts  rechargeable  lead  acid  batteries  [Ref  1].  It  is  designed  to  maintain  a  top 
speed  of  3  -  4  knots  for  3  hours  and  can  operate  safely  at  a  depth  of  30  meters.  Main 
propulsion  is  achieved  using  twin  0.5  HP  electric  drive  thrusters  located  at  the  stern  of  the 
vehicle.  During  normal  flight  operation,  depth  dive  and  heading  are  controlled  by  a 
combination  of  upper  bow,  stem  radders,  bow  planes,  and  stem  planes. 

Various  onboard  sensors  serve  to  either  collect  data  or  provide  necessary 
information  to  the  AUV  control  systems.  These  sensors  are: 

•  Depth  cell  and  Systron  Dormer  3  (IMU)  MotionPak 

•  Tri-tech  ST725  (scanning  sonar  for  obstacle  avoidance  and  target 
acquisition/reacquisition) 

•  1200  kHz  RD  instmments  navigator  Doppler  Velocity  Log  that  contains  a 
TCM  2  magnetic  compass  (navigational  sensors) 
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•  Kearfort  KG  2000  Gyro  system 

•  Honeywell  HG  3000  magnetorestmetive  eompass 
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Figure  3.  Naval  Postgraduate  School  AUV  Aries. 
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III.  EXPERIMENTAL  TESTING 


A.  GENERAL 

Faculty  members  from  the  Graduate  School  of  Engineering  and  Applied  Science, 
the  Institute  of  Superior  Technico  (1ST),  and  the  University  of  Azores  have  had  a  long 
standing  agreement  for  the  exchange  of  technology  concerning  AUV.  The  three  schools 
have  shared  effort  on  the  evaluation,  research,  system  methodologies,  and  mission 
specifications  of  the  AUV.  An  experimental  mission  was  conducted  around  the  island  of 
Faial  in  the  Azores  during  August  2001  comparing  ARIES  and  DELFIM.  The 
experiments  facilitated  analysis  of  the  underwater  navigation  system,  their  ability  to 
follow  the  preprogrammed  tracks,  and  acoustic  communication  links  for  command  and 
data  transfer  between  the  two  vehicles.  Professor  Anthony  Healey,  Dr.  Dave  Marco,  and 
Mr.  Robert  Dayak  from  NPS  flew  to  the  Azores  in  early  August  2001  with  the  AUV 
Aries.  The  vehicle  has  a  video  capability  using  a  deep  sea  power  light  multi  seacam 
camera,  and  an  acoustic  modem  for  underwater  communications  using  a  new  modem 
under  development  at  Florida  Atlantic  University.  Diving  support  and  surface  ship 
support  were  provided  by  the  University  of  Azores  and  Azores  Department  of  Fisheries. 
The  second  autonomous  underwater  vehicle  named  DELFIM  was  provided  by  the 
University  of  Lisbon.  For  the  experimental  mission,  a  pair  of  acoustic  modems  was 
installed  on  the  ARIES  and  DELFIM.  Two  laptop  computers  were  used  on  the  vessel, 
Arquipelago,  which  served  as  the  base  station  for  this  exercise.  Communications  were 
directed  from  the  surface  ship  Arquipelago  to  the  surface  AUV  DELFIM  which,  in  turn, 
transferred  data  to  the  submerged  AUV  ARIES  through  an  acoustic  modem  mounted 
below  the  AUV  DELFIM.  Figure  4  shows  the  map  of  the  Azores  operational  area. 
ARIES  being  loaded  onto  the  vessel  Arquipelago  is  showed  in  Figure  5,  and  ARIES  at 
one  meter  depth  operated  by  the  research  vessel  Arquipelago  is  showed  in  Figure  6. 
ARIES  and  DELFIM  at  the  operational  area  are  seen  from  the  vessel  Arquipelago  in 
Figure  7. 
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Figure  4.  Map  of  the  Island  of  Faial,  Azores-Operational  Area  Northeast  of  Horta. 


Figure  5.  ARIES  being  Loaded  onto  the  Research  Vessel  Arquipelago  (August  09,  2001). 
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Figure  6.  Arquipelago  and  ARIES  -  at  1  Meter  Depth  on  August  12,  2001. 


Figure  7.  ARIES  and  DELFIM  at  Operational  Area. 
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B.  ACTIVITIES  CONDUCTED 


Day-to-day  activities  are  listed  in  Table  1  below. 


Date 

Activities 

Monday,  August  6 

Arrived,  unpack  Aries 

Tuesday,  August  7 

Partial  re-assembly  of  Aries 

Wednesday,  August  8 

System  connections  and  testing 

Thursday,  August  9 

Acoustic  modem  software  integration  and 
testing 

Friday,  August  10 

Deployment  of  Aries  to  Arquipilago  Modem 
test  and  evaluation  in  harbor  ballast  tests  in 
harbor.  Transit  to  site,  mission  0 

Saturday,  August  1 1 

Transit  to  site  navigation  tests  of  Aries  at  site, 
development  of  new  deviation  table;  Mission 
1,  2,  3  -  Navigation 

Sunday,  August  12 

Missions  4,5,6.  Missions  7,8,9 

Monday,  August  13 

Off-load  Aries 

Tuesday,  August  14 

Pack  up  Aries  for  transportation 

Wednesday,  August  15 

Pack  up  Aries  for  transportation 

Table  1.  List  of  Daily  Activity. 

Files  obtained  for  navigational  accuracy  studies  are  given  in  Table  2  below. 


Data  File 

Comments 

Mission  0;  d081001  Ol.d 

In  harbor  Communications  Tests 

Mission  l;d08 1101  Ol.d 

Navigation  test 

Mission  2;  d081101_02.d 

Navigation  test 

Mission  3 ;  d08 1 1 0 1  03  .d 

Navigation  test 

Mission  4;  d081201_01.d 

Video  acquisition  tests 

Mission  5;  d081201_02.d 

Video  acquisition  tests 

Mission  6;  d081201_03.d 

Video  acquisition  tests 

Mission  7;  d081201  04.d 

Acoustic  communications  tests 

Mission  8;  d081201_05.d 

Acoustic  communications  tests 

Mission  9;  d081201_06.d 

Acoustic  communications  tests 

Table  2.  List  of  Navigation  Files  and  Tracks. 

C.  DATA  COLLECTION 

After  initializing  the  Aries  and  Delfim  computers  and  allowing  the  DG  to  spin  up 
and  stabilize,  the  research  vessel  Arquipelago  launched  the  AUVs  into  the  operational 
area.  The  AUV  research  group  conducted  several  tests  with  the  ARIES  and  DELFIN  to 
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establish  communication  between  all  three  vehicles.  Communications  between  all  three 
vehicles  were  tested  and  the  results  were  satisfied.  Data  was  then  collected  and  stored  at  a 
sampling  rate  of  8  hertz  for  all  missions.  These  data  are  useful  for  analysis  of  navigation 
error  and  comparison  of  actual  vehicle  tracks  with  preprogrammed  tracks  as  described  in 
Chapters  IV  and  V. 
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IV.  METHODOLOGIES  OF  DATA  ANALYSIS 


A.  NAVIGATION  ERROR 

The  AUV  ARIES  uses  an  inertial  navigation  system,  Doppler,  DGPS  navigational 
suite  and  an  extended  Kalman  filter  [Ref.  10],  This  navigation  system  may  be  tuned  for 
optimal  performance  given  a  set  of  data.  The  Kalman  filter  integrates  data  from  different 
sensors  such  as  velocity,  heading,  and  speed  over  ground,  etc.  The  complexity  of  the 
Kalman  filter  resides  in  an  8-state  non-linear  model  of  vehicle  motion.  The  Kalman  filter 
optimally  integrates  data  and  recursively  processes  the  measurements  to  improve  system 
performance.  The  most  important  sensors  are  those  for  speed  over  ground  and  heading 
reference.  The  heading  reference  is  derived  from  both  the  compass,  located  in  the  RDI 
navigator,  and  the  Systron  Donner  (IMU),  which  provides  yaw  rate.  The  combination 
fusion  of  the  yaw  rate  and  the  compass  data  leads  to  an  identification  of  the  yaw  rate  bias, 
which  is  assumed  to  be  a  constant  value.  The  sensors  provide  input  to  the  vehicle  control 
system.  When  the  vehicle  is  submerged,  the  heading  bias  is  unobservable  and  will 
continue  to  grow  until  a  new  update  is  obtained  [Ref  5].  When  the  vehicle  surfaces  for  a 
short  time  of  approximately  ten  seconds,  the  Kalman  filter  and  new  GPS  update  serve  to 
correct  the  estimation  of  all  states.  This  position  error  is  corrected  by  allowing  the  filter  to 
re-estimate  biases  and,  thereby,  improve  accuracy.  The  AUV  Aries  was  operated  in  the 
Azores  from  August  10-12,  2001  in  a  series  of  runs  that  included  a  dive-surface-dive- 
surface  sequence.  Data  was  retrieved  from  the  file  and  analyzed  for  position  error  when 
the  vehicle  was  submerged.  While  the  AUV  Aries  was  submerged,  it  was  unable  to 
receive  GPS  signals  from  all  the  available  orbiting  satellites.  Therefore,  it  utilized  its  own 
Kalman  filter  navigation  system  to  determine  the  vehicle  location  at  all  time  [Ref  6]. 
When  the  AUV  Aries  surfaced  after  a  dive,  it  received  GPS  signals  from  all  the  available 
satellites.  The  Kalman  filter  re-estimated  biases,  corrected  position  estimates  and 
continued  to  improve  accuracy.  For  the  mission  run  on  August  11,  2001,  data  from  six 
dive-surface  operations  were  extracted  from  the  vehicle’s  internal  file.  Figures  8  through 
13  compare  vehicle  locations  obtained  from  GPS  with  locations  calculated  from  the 
vehicle’s  internal  navigation  system.  The  differences  in  location  are  shown  in  meters  in 
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the  Y-axis  ant  the  time  in  second  in  the  X-axis.  Total  submerged  time  for  the  AUV  Aries 
varied  between  87  to  546  seconds. 
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Figure  8.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  Surface  on  August  11,  2001  (b)  Differences  between  GPS  and  Internal  Navigation 
in  the  East/West  Direction  for  the  1®'  Surface  on  August  11,  2001. 
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(b) 

Figure  9.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  2“*^  Surface  on  August  11,  2001  (b)  Differences  between  GPS  and  Internal 
Navigation  in  the  East/West  Direction  for  the  2"‘*  Surface  on  August  11,  2001. 
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Figure  10.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  3"^^*  Surface  on  August  11,  2001  (b)  Differences  between  GPS  and  Internal  Navigation 
in  the  East/West  Direction  for  the  3'^'*  Surface  on  August  11,  2001. 
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Figure  11.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  4*  Surface  on  August  11,  2001  (b)  Differences  between  GPS  and  Internal  Navigation 
in  the  East/West  Direction  for  the  4*'^  Surface  on  August  11,  2001. 
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Figure  12.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  5*  Surface  on  August  11,  2001  (b)  Differences  between  GPS  and  Internal  Navigation 
in  the  East/West  Direction  for  the  5*'^  Surface  on  August  11,  2001. 
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Figure  13.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  6*  Surface  on  August  11,  2001  (b)  Differences  between  GPS  and  Internal  Navigation 
in  the  East/West  Direction  for  the  6*  Surface  on  August  11,  2001. 
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For  the  mission  run  on  August  12,  2001,  data  from  seven  dive-surface  operations 
were  extracted  from  the  vehicle’s  internal  file.  Figures  14  through  20  compare  vehicle 
locations  obtained  from  GPS  with  locations  calculated  from  the  vehicle’s  internal 
navigation  system.  The  differences  in  location  are  shown  in  meters  in  the  Y-axis  ant  the 
time  in  second  in  the  X-axis.  Total  submerged  time  for  the  AUV  Aries  varied  between  36 
to  427  seconds. 
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Figure  14.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  1®‘  Surface  on  August  12,  2001  (b)  Differences  between  GPS  and  Internal  Navigation 
in  the  East/West  Direction  for  the  1®‘  Surface  on  August  12,  2001. 
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Figure  15 


(b) 

(a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 
the  2“'^  Surface  on  August  12,  2001  (b)  Differences  between  GPS  and  Internal 
Navigation  in  the  East/West  Direction  for  the  2"^*  Surface  on  August  12,  2001. 
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Figure  16.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  3"^^*  Surface  on  August  12,  2001  (b)  Differences  between  GPS  and  Internal  Navigation 
in  the  East/West  Direction  for  the  3'^'*  Surface  on  August  12,  2001. 


33 


lij 


(a) 


34 


(b) 

Figure  17.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  4*  Surface  on  August  12,  2001  (b)  Differences  between  GPS  and  Internal  Navigation 
in  the  East/West  Direction  for  the  4*  Surface  on  August  12,  2001. 
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Figure  18.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  5*  Surface  on  August  12,  2001  (b)  Differences  between  GPS  and  Internal  Navigation 
in  the  East/West  Direction  for  the  5*  Surface  on  August  12,  2001. 
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Figure  19.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  6*  Surface  on  August  12,  2001  (b)  Differences  between  GPS  and  Internal  Navigation 
in  the  East/West  Direction  for  the  6*  Surface  on  August  12,  2001. 
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Figure  20.  (a)  Differences  between  GPS  and  Internal  Navigation  in  the  North  Direction  for 

the  7*  Surface  on  August  12,  2001  (b)  Differences  between  GPS  and  Internal  Navigation 
in  the  East/West  Direction  for  the  7*'^  Surface  on  August  12,  2001. 
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Table  3  consolidates  all  thirteen  dive  surface  operations  and  shows  the  total 
distance  of  track  R. 


Date 

From 

To 

Total  time 
vehicle 
submerged 
(seconds) 

Y 

(m) 

X 

(m) 

R 

(m) 

#  of 
sat. 

Hdop 

08110102 

311 

398 

87 

2.0 

0.2 

2.01 

6 

1.4 

413 

582 

169 

2.8 

2.2 

3.56 

8 

2.0 

596 

703 

107 

4.0 

13.0 

13.6 

3 

7.6 

1237 

1730 

493 

17.0 

11.0 

20.25 

8 

1.8 

2644 

3190 

546 

9.8 

7.2 

12.16 

8 

3.1 

08110104 

839 

1266 

427 

11.0 

11.0 

15.56 

6 

4.1 

08120102 

262 

298 

36 

1.5 

0.9 

1.75 

7 

1.4 

306 

343 

37 

1.7 

4.0 

4.35 

6 

6.9 

352 

477 

125 

2.0 

4.0 

4.97 

3 

6.1 

2712 

2811 

99 

5.3 

3.2 

6.19 

4 

8.3 

2811 

3138 

327 

5.5 

13.0 

14.12 

2 

3.1 

4524 

4578 

54 

7.5 

14.0 

15.88 

3 

8.2 

4578 

4838 

260 

11.4 

6.3 

13.02 

5 

1.6 

Table  3.  Differences  between  GPS  and  Underwater  Vehicle  Navigation  System  during 

Surfaces  from  Aug  10-12,  2001. 


The  following  three  figures  show  that  the  position  errors  increase  as  the  vehicle 
submersion  time  increases. 

In  Figure  21,  a  close  up  look  at  the  difference  between  the  Kalman  fdter  solution 
and  the  DGPS  data  point  in  meters  versus  submerged  time  in  seconds  while  AUV  Aries 
surfaced  for  all  thirteen  dive-surface  operations.  The  least  square  method  was  utilized  to 
calculate  the  graph  that  best  fitted  all  data  points  with  the  line  going  through  the  zero 
point  origin.  The  MATLAB  code  for  the  least  square  method  with  the  line  going  through 
the  origin  is  listed  in  Appendix  A. 
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Navigation  Filter  Error  Versus  Submerged  Time 


Figure  21.  Difference  between  the  Kalman  Filter  Solution  and  the  DGPS  Data  Point  in 

Meters  Versus  Submerged  Time  in  Seconds. 

In  Figure  22,  took  into  consideration  that  DGPS  is  more  accurate  if  there  were  at 
least  four  satellites  were  being  used  to  compute  the  position.  Therefore,  DGPS  data  with 
less  four  satellites  were  drop  off.  Again,  the  least  square  method  was  utilized  to  calculate 
the  graph  that  best  fitted  all  data  points  with  the  line  going  through  the  zero  point  origin. 
The  MATLAB  code  for  the  least  square  method  with  the  line  going  through  the  origin  is 
listed  in  Appendix  B. 
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Navigation  Filter  Error  Versus  Submerged  Time  For  NSv  More  Than  Four 


Figure  22.  DGPS  More  Accurate  with  at  least  Four  Satellites  to  Compute  the  Position. 

In  Figure  23,  took  into  consideration  of  the  Horizontal  dilution  of  precision 
(Hdop)  values.  Values  of  Hdop  between  1.2  and  1.7  are  usually  associated  with  high 
precision.  This  value  is  a  good  figure  of  merit.  Therefore,  DGPS  data  with  a  Hdop  values 
of  less  than  1.2  or  more  than  2.0  were  dropped  off.  Again,  the  least  square  method  was 
utilized  to  calculate  the  graph  that  best  fitted  all  data  points  with  the  line  going  through 
the  zero  point  origin.  The  MATLAB  code  for  the  least  square  method  with  the  line  going 
through  the  origin  is  listed  in  Appendix  C. 
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Navigation  Filter  Error  Versus  Submerged  Time  For  Hdop  Less  Than  Two 


Figure  23 .  Horizontal  Dilution  of  Precision  (Hdop)  Values. 

B.  TRACK  FOLLOWING  ALGORITHM 

The  AUV  Aries  currently  uses  four  different  automatic  pilots  for  flight 
maneuvering  control:  diving,  altitude  above  bottom,  steering,  and  cross  track  error 
controllers  [Ref  1],  These  four  automatics  pilots  are  based  on  sliding  mode  control 
theory.  Each  of  the  four  modes  is  de-coupled  for  ease  of  implementation  and  design. 
[Ref  7]  provides  the  details  of  controller  design.  Sliding  mode  controllers  are  chosen 
over  fuzzy  and  heuristic  controllers  because  they  are  simple  to  use  and  implement  with 
minimal  tuning  [Ref.  8]. 

1.  Heading  Controller 

A  second  order  model  equation  is  used  to  control  the  vehicle  heading  and 
eliminates  the  need  to  feed  back  the  sideslip  velocity.  The  effects  of  sideslip  are  treated  as 
disturbances  that  the  controller  must  overcome.  Therefore,  the  heading  model  equation 
becomes: 
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r{t)  =  ar(t)  +  b5^  {t)  +  disturbances 


(1) 


¥it)  =  r{t)  (2) 

where  y/{t)  is  the  vehicle  heading  angle,  r{t)  is  the  yaw  rate,  and  5^{t)  is  the  stem 

mdder  angle.  The  coefficients,  a  and  b ,  have  been  determined  from  experiments;  and 
they  are  -.30/sec  and  -.1 125/sec^2  respectively.  The  stem  and  bow  mdders  operate  in  the 
same  way  as  the  planes,  therefore,  the  command  to  the  bow  mdder  is  -5^  (t) .  In  order  to 

use  this  steering  law,  the  heading  error  -^(t))must  lie  between  ±180®  and  is  de- 

wrapped  as  needed  in  order  to  make  this  happen.  By  ignoring  any  non-zero  command,  the 
sliding  surface  is  defined  by 

a(t)  =  -.9499K0  +  .1V01(?/^_  -i/^(t))  (3) 

The  stem  mdder  command  for  heading  control  is  defined  by 

5,  (0  =  -1 .543(2.5394r(0  +  //  tanh(cr(t)  /  (/)))  (4) 

where  77=1.0  and =0.5. 

2.  Cross  Track  Error  Controller 

To  follow  a  set  of  predicted  straight  line  tracks  from  a  simulation  model  of  the 
vehicle  track  following  behavior  [Ref  1],  a  sliding  mode  controller  is  presented  that  has 
been  experimentally  validated.  Other  works  have  been  studied  for  this  type  of  problem 
[Ref  9]  usually  developing  a  stable  guidance  law  based  on  cross  track  error.  Utilizing 
Figure  24  as  a  guide,  we  use  a  combination  of  cross  track  error  control  and  line  of  sight 
control.  Cross  track  error  control  cannot  be  guaranteed  stable  with  large  error  heading, 
while  line  of  sight  control  will  reduce  heading  errors  to  zero.  Switching  between  these 
two  controllers  allows  for  stable  control  and  reduction  in  heading  errors  under  all 
conditions. 
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Figure  24.  Track  Geometry  and  Velocity  Vector  Diagram. 


The  variable  of  interest  to  minimize  is  the  cross  track  error,  s{t) ,  and  is  defined  as 
the  perpendicular  distance  between  the  center  of  the  vehicle  located  at  V(t),T(t)and  the 
adjacent  track  line.  The  total  track  length  between  way  point  i  and  /  - 1  is  given  by 


wpt{i)  ^ ^^wpt(i)  ^wpt{i-\)^ 


(5) 


where  the  pairs  and  are  the  current  and  previous  way  points 

respectively.  The  track  angle,  ,  is  defined  by 


=arctan2(T„^,(.,- 

^wpt{i-\)  ’  ^wpt(i)  ^wpt(i-l)  )  (6) 

□ 

and  is  constant  for  a  given  set  of  adjacent  way  points.  Cross  track  heading  error,  > 

for  the  /'*  segment  is  defined  by 

□ 

¥{t)cteii)=¥{t)-¥trm  (7) 

□ 

where  must  be  normalized  to  stay  between  ±180°.  The  difference  between  the 

current  vehicle  position  and  the  next  waypoint  is 
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(8) 


With  the  above  definition,  the  distanee  to  the  way  point  projeeted  to  the  traek 
line,  S{t)  ean  be  defined  as 

s{t), = no.,,0  ]•  )]  /  A-  •  (9) 

5'(t)  ranges  from  0-100%  ofZ. 

The  cross  track  error,  s{t) ,  may  now  be  defined  as 

f(0  =  5(0,sin(^/^(0)  (10) 

where  (?)  is  the  angle  between  the  line  of  sight  to  the  next  way  point  and  the  current 
track  line,  given  by 

dp (0  =  arctan 2(7^^,^ -  7„^,( ._i, , )  -  arctan 2(7 (?)„^,( .^ , X (t)^^,^ )  . 

(11) 

and  (i^(?)  must  be  normalized  to  lie  between  ±180®,  and  arctan2  is  the  inverse  tangent 
function  atan2,  as  defined  in  MATLAB  language. 

With  the  cross  track  error  defined,  the  sliding  surface  can  be  cast  in  terms  of 
derivatives  of  the  errors  such  that. 

s{t)  =  Ur{t)  cos{co{t)^^^ )  (pi  >  2; 

e{t)  =  U r{t)co^{co{t)^J-Ur{tf  sin(6i(?)^,^(,.,) 

where  U  is  the  nominal  longitudinal  speed  of  the  vehicle.  The  sliding  surface  for  the  cross 
track  error  controller  becomes  a  second  order  polynomial  of  the  form 
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(12) 


(j{t)  =  s{t)  +  \  s{t)  +  X2S{t) 

The  condition  for  stability  of  the  sliding  mode  controller  is 

<j{t)  =  s{t)  +  Tj  s{t)  +  Tj  sit)  =  -rj^crit) !  (p),  (13) 

and  to  recover  the  input  for  control,  the  heading  dynamics,  equation  (1),  may  be  used  into 
equation  (12)  to  yield 

(14) 

By  rewriting  equation  (12),  the  sliding  surface  becomes 

(Tit)  =  f/r(0cos(®(0,,,(,))  +  \u  sin(n;(0,,,(o)  +  ^2^(0-  (15) 

the  rudder  input  can  be  expressed  as 

f  \ 

5r (0  =  - 1 -  i-Uarit) cos(®(0,,,(,) )  +  U irit)f  sin(®(0,,,(,) )  -  Tit/r(0cos((y(0 ) 

^C/Z)cos(®(0,, ,(,)), 

-X^U sin(®(t)^,^(.)  -rii(7it)l(p)y,  (16) 

where  X^=0.6,  ^2=0.1,  7=0.1,  and  ^3=0. 5.  To  avoid  division  by  zero,  in  the  rare  case 
where  cos(&)(t)^^g^,j)  =  0(  vehicle  heading  is  perpendicular  to  the  track  line)  the  rudder 

command  is  set  to  zero  since  this  condition  is  transient  in  nature.  If  co{t) ! 2 ,  then 

the  vehicle  will  follow  the  track,  but  travel  in  the  opposite  direction  to  that  desired.  In 
order  to  prevent  this  from  happening  in  practice,  a  bound  of  40  degrees  is  used  as  a 
switch  to  light  of  sight  control. 

3.  Line  of  Sight  Controller 

When  the  condition  arises  that  the  magnitude  of  the  cross  track  heading  error 
exceeds  40  degrees,  a  line  of  sight  is  used  and  the  heading  command  can  be  determined 
from 
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(17) 


□  □ 

y^^^com{LOS)  arctan  2(7  (0„^,(,.) ,  X  (0„^,(,.) ) , 

and  the  line  of  sight  error  from 

□ 

¥{t)LOS  =  ¥{t)com(LOS)  "  V'CO  (18) 

The  laws  used  for  heading  control,  equations  (3,  4),  may  be  used.  When  the 
mission  begins,  the  initial  heading  of  the  vehicle  is  seldom  aligned  with  the  command 
track.  Line  of  sight  control  forces  the  vehicle  to  head  in  the  direction  of  the  current  way 
point  and  once  the  cross  track  heading  error  falls  below  40  degrees,  cross  track  control  is 
used.  In  the  second  scenario,  when  the  angle  between  two  sequential  tracks  lines 
exceeded  40  degrees,  two  conditions  may  be  true  for  the  waypoint  index  to  be 
incremented.  The  first  would  be  if  the  vehicle  has  penetrated  the  waypoint  watch  radius, 
R,  which  is  set  at  2  meters  for  this  track  following  model.  The  second  would  be  if  a  large 
amount  of  cross  track  error  is  presented.  In  that  case,  the  next  way  point  would  become 
active  if  the  projected  distance  to  the  way  point,  reached  such  that  if 

I  □  □ 

then  activate  the  next  way  point. 

C.  COMPARISON  OF  DESIGN  TRACKS,  MODEL  PREDICTION  TRACKS 

AND  ACTUAL  TRACKS 

Using  the  track  prediction  program  called  New-CTE-Box  pattern,  a  box  search 
pattern  was  used  to  circle  the  predicted  autonomous  underwater  vehicle  target  points.  The 
MATLAB  code  for  the  New-CTE-Box  pattern  is  included  as  Appendix  D.  Twenty-four 
track  lengths  and  twenty-four  turning  points  were  entered  into  the  model  prediction  tracks 
program.  The  twenty-four  points  are  a  result  of  a  model  AUV  Aries  running  in  the  same 
square  box  pattern  six  times.  The  same  preprogrammed  tracks  and  turning  points  were 
also  loaded  into  the  AUV  Aries  during  the  mission  runs  in  the  Azores  on  August  10-12, 
2001.  However,  the  AUV  Aries  executed  only  twelve  track  lengths  and  twelve  tuning 
points,  which  is  equivalent  to  running  the  same  square  box  pattern  three  times.  Data 
collected  from  the  mission  were  stored  and  loaded  into  the  track  prediction  program  as 
experimental  values  of  the  actual  tracks.  Simulations  of  the  AUV  Aries  run  was 
performed  by  the  prediction  program  based  on  similar  conditions.  Figure  25  shows  the 
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differences  in  the  design  tracks,  model  prediction  tracks  and  actual  tracks.  Analysis  and 
comparison  show  that  the  actual  tracks  differ  from  the  design  tracks  by  margins  of 
between  2  to  15  meters.  This  error  is  due,  among  other  reasons,  to  rudder  response  time, 
forward  motion,  sway,  yaw,  cross  track  error  steering  and  line  of  sight  steering.  Like  an 
automobile,  it  is  impossible  for  the  AUV  Aries  to  turn  a  90  degrees  comer  while  in 
continuous  motion.  The  prediction  tracks  and  the  actual  tracks  are  very  similar  in  shape. 
The  differences  between  actual  tracks  and  prediction  tracks  (2  to  4  meters)  are  smaller 
than  the  differences  between  actual  and  design  tracks.  More  importantly,  model  tracks 
reach  a  steady  state  after  the  first  loop.  This  convergence  demonstrates  that  the  prediction 
program  works.  At  the  very  end  of  the  mn,  the  AUV  Aries  aborted  and  surfaced.  The 
prediction  program  does  not  simulate  the  end  of  the  ran  where  the  AUV  Aries  finished 
and  surfaced. 


Design  tracks,  model  prediction  tracks,  and  actual  tracks 


Figure  25.  Design  Tracks,  Model  Prediction  Track  and  Actual  Tracks  with  Simulated  No 

Current. 
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Figure  26,  utilizing  the  same  aetual  traeks  and  design  traeks,  introduces  a 
simulated  current  in  the  south  direction  at  0.5  knots  for  the  prediction  model.  The  design 
tracks  and  actual  tracks  remain  the  same  as  in  Figure  25.  However,  the  model  prediction 
tracks  reached  steady  state  and  are  closer  to  the  actual  tracks.  This  indicates  that  the 
ocean  current  was  in  the  southern  direction  during  the  mission  run. 


Design  tracks,  model  prediction  tracks,  and  actual  tracks 


Figure  26.  Design  Tracks,  Model  Prediction  Track  and  Actual  Tracks  with  Simulated  South 

Current. 

Figure  27  introduces  a  simulated  current  in  the  north  direction  at  0.5  knots.  The 
design  tracks  and  actual  tracks  remain  the  same.  The  prediction  tracks  reached  a  steady 
state  but  were  shifted  in  the  direction  of  the  current,  and  thus  increased  the  separation 
between  the  actual  tracks  and  the  prediction  tracks.  This  further  indicates  that  the  current 
was  in  the  southern  direction  at  the  time  of  the  mission  run. 
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Design  tracks,  model  prediction  tracks,  and  actual  tracks 


Figure  27.  Design  Tracks,  Model  Prediction  Track  and  Actual  Tracks  with  Simulated  North 

Current. 

Figure  28  introduces  a  simulated  current  in  the  east  direction  at  0.5  knots.  The 
prediction  tracks  reached  a  steady  state  and  are  closer  to  the  actual  tracks.  This  indicates 
that  the  ocean  current  was  in  the  easterly  direction  during  the  mission  run. 
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Design  tracks,  model  prediction  tracks,  and  actual  tracks 


Figure  28.  Design  Tracks,  Model  Prediction  Track  and  Actual  Tracks  with  Simulated  East 

Current. 

Lastly,  Figure  29  introduces  a  simulated  current  in  the  west  direction  at  0.5  knots. 
The  actual  tracks  and  designed  tracks  remain  unchanged.  The  prediction  tracks  reached  a 
steady  state  but  were  shifted  in  the  direction  of  the  current,  which  increased  the 
separation  between  the  actual  tracks  and  the  prediction  tracks.  This  also  indicated  that  the 
direction  of  the  ocean  current  was  in  the  easterly  direction.  Based  on  these  four  imposed 
current  conditions,  it  was  determined  that  the  direction  of  the  current  was  somewhere  in 
the  southeast  direction. 
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Design  tracks,  model  prediction  tracks,  and  actual  tracks 


Figure  29.  Design  Tracks,  Model  Prediction  Track  and  Actual  Tracks  with  Simulated  West 

Current. 

Figure  30  shows  a  current  of  0.5  knots  in  the  south  and  0.5  knots  in  the  east.  The 
prediction  tracks  again  reached  a  steady  state  and  are  very  close  to  the  actual  tracks. 
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Design  tracks,  model  prediction  tracks,  and  actual  tracks 


Figure  30.  Design  Tracks,  Model  Prediction  Track  and  Actual  Tracks  with  Simulated 

Southeast  Current. 
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V.  CONCLUSIONS 


A.  SUMMARY 

Several  factors  are  of  primary  concern  for  Autonomous  Underwater  Vehicles 
(AUV)  as  a  viable  platform  for  missions  such  as  minefields,  reconnaissance,  ocean  floor 
surveys,  and  decision  making  in  the  conduct  of  amphibious  operations  in  shallow  water. 
These  factors  include  reliability,  accuracy,  and  a  high  precision  navigation  system  for  its 
submerged  operations.  Ocean  floor  data  collected  for  decision  making  can  be  meaningful 
if,  and  only  if,  the  precise  position  of  the  vehicle  is  known  at  the  time  the  information  is 
recorded.  The  challenge  of  obtaining  a  highly  accurate  vehicle  position  during  submerged 
navigation  is  ever  present.  This  thesis  analyzed  the  navigation  errors  during  submerged 
operations  of  the  Autonomous  Underwater  Vehicle  Aries  on  August  10-12,  2001. 
Navigation  errors  were  computed  as  a  function  of  the  distance  the  AUV  Aries  traveled 
while  submerged.  These  errors  also  take  into  consideration  the  number  of  satellites  that 
were  available  to  the  AUV  Aries  during  pop-up  maneuvers  and  the  figure  of  merit  of  the 
Global  Positioning  System  (GPS).  Model  prediction  tracks  with  and  without  current 
conditions  were  simulated  in  MATLAB  and  compared  to  the  actual  tracks.  Analysis  of 
the  differences  between  the  two  tracks  provided  insight  into  the  cross  track  error  steering, 
line  of  sight  steering  and  rudders  response  time. 

B.  RESULTS 

Navigation  errors  for  the  AUV  Aries  during  the  Azores  operations  was  found  to 
be  anywhere  between  2.22%  to  2.72%  of  the  distance  the  Aries  traveled  while  the  vehicle 
was  submerged. 

•  Error  is  based  on  the  analysis  of  thirteen  pop-up  maneuvers  and  the 
comparison  of  the  distance  differences  between  the  GPS  and  the  vehicles’ 
own  internal  navigation  system.  Error  was  found  to  be  2.44%  of  the 
distance  traveled.  All  data  points  were  scattered  around  the  straight  line. 

•  Error  is  based  on  the  analysis  of  eight  pop-up  maneuvers  and  comparison 
of  the  distance  differences  between  the  GPS  and  the  vehicles’  own  internal 
navigation  system.  Five  pop-up  maneuvers  were  eliminated  because  the 
number  of  satellites  available  at  the  time  of  the  pop-up  was  less  than  three. 
Error  was  found  to  be  2.44%  of  distance  traveled. 

•  Error  is  based  on  the  analysis  of  five  pop-up  maneuvers  and  the 
comparison  of  the  distance  differences  between  the  GPS  and  the  vehicles’ 
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own  internal  navigation  system.  Eight  pop-up  maneuvers  were  eliminated 
because  the  GPS  figure  of  merit  was  too  high.  Error  was  found  to  be  2.72 
of  distance  traveled.  All  data  points  fitted  with  the  line  almost  perfectly. 

C.  RECOMMENDATIONS 

To  record  meaningful  data  sets,  it  is  necessary  to  have  an  accurate  vehicle 
location  at  the  time  information  is  recorded.  When  continually  conducting  missions  such 
as  minefield,  reconnaissance,  mine  identification,  and  mine  sweeping  on  enemy 
shorelines,  the  AUV  must  be  able  to  operate  independently  and  covertly  to  avoid 
detection  by  enemies.  Unlike  the  former  Soviet  Union,  our  current  and  future  adversary’s 
coastal  defenses  consist  of  low  technology  surface  search  radar,  observation  posts  and 
defensive  mine  warfare.  Continuous  surfacing  by  the  AUV  to  receive  GPS  signals  and 
correct  vehicle  position  could  result  in  enemy  detection  from  their  observation  posts.  The 
decision  making  process  in  the  conduct  of  our  amphibious  operations  in  shallow  water 
depends  on  several  factors  such  as  combat  air  support  superiority.  Naval  gunfire  support, 
and  amphibious  landing  troops  and  equipment.  Our  adversaries  cannot  match  the  United 
States  in  terms  of  air  superiority  and  Naval  gunfire  support,  but  realize  that  defensive 
mine  warfare  is  their  advantage.  Mines  carry  great  destructive  power  even  though  they 
are  cheap  to  produce  and  do  not  require  sophisticated  technology.  The  current  AUV  Aries 
navigation  system  error  of  approximately  2.22%  of  distance  traveled  while  submerged 
may  be  unacceptable  for  long  submerged  operations.  A  higher  precision  navigation  gyro 
compass,  costing  approximately  $75,000  per  unit,  could  reduce  errors  to  1%  of  distance 
traveled. 

The  vehicle  control  system  should  have  a  means  to  account  for  current  conditions. 
Future  work  should  include  an  investigation  of  a  method  for  calculating  current 
conditions  then  continuously  feeding  them  into  the  feedback  control  loop.  The  AUV 
would  then  be  able  to  adjust  its  bow  and  stern  rudders  as  necessary  to  compensate  for  set 
and  drift  in  order  to  maintain  course  and  speed.  This  would  be  a  revolutionary  approach 
for  AUV  control  and  navigation  systems,  which  would  minimize  navigation  errors  and 
allow  the  vehicle  to  stay  submerged  longer.  The  vehicle  could  avoid  continuous  surfacing 
to  update  the  navigation  fdter  with  DGPS  and  enemy  detection  observation  posts.  With 
its  continuous  feedback  loop  to  adjust  the  vehicle  bow  and  stem  mdders,  the  AUV  could 

maintain  its  intended  tracks.  Our  amphibious  landing  planning  team  could  divide  the 
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enemy’s  shoreline  into  sea  lanes.  The  width  of  the  lane  would  depend  on  the  range  of  the 
vehicle  camera.  The  AUV  could  operate  inside  its  lanes  without  worrying  about  current 
conditions.  Information  could  be  recorded  and  sent  back  to  the  mother  platform  for  a 
decision  from  our  amphibious  landing  forces. 
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APPENDIX  A.  MATLAB  CODE  FOR  NAVIGATION  ERROR 

VERSUS  TIME 


t=[0  87  169  107  493  546  427  36  37  125  99  327  54  260]; 

r=[0  2.01  3.56  13.6  20.25  12.16  15.56  1.75  4.35  4.97  6.19  14.12  15.88  13.02]; 

plot(t,r,'*') 

hold 

num=sum(t.*r); 

den=sum(t.^2); 

m=num/den; 

P=[m0]; 

rl=polyval(P,t); 

plot(t,rl) 

title('Navigation  Filter  Error  Versus  Submerged  Time') 
xlabel('Time  in  Seconds') 
ylabel('Distance  in  Meters') 
grid 

gtext('error=2.44%  distance  travel') 
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APPENDIX  B.  MATLAB  CODE  FOR  NAVIGATION  ERROR 
VERSUS  TIME  FOR  NSV  MORE  THAN  FOUR 


=[0  87  169  493  546  427  36  37  260]; 

r=[0  2.01  3.56  20.25  12.16  15.56  1.75  4.35  13.02]; 

plot(t,r,'*') 

hold 

num=sum(t.*r); 

den=sum(t.^2); 

m=num/den; 

P=[m0]; 

rl=polyval(P,t); 

plot(t,rl) 

titleCNavigation  Filter  Error  Versus  Submerged  Time  For  NSv  More  Than  Four') 
xlabel('Time  in  Seconds') 
ylabel('Distance  in  Meters') 
grid 

gtext('error=2.22%  distance  travel') 
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APPENDIX  C.  MATLAB  CODE  FOR  NAVIGATION  ERROR 
VERSUS  TIME  HDOP  BETWEEN  1.2  -  1.7 


t=[0  87  169  493  36  260]; 

r=[0  2.01  3.56  20.25  1.75  13.02]; 

plot(t,r,'*') 

hold 

num=sum(t.*r); 

den=sum(t.^2); 

m=num/den; 

P=[m0]; 

rl=polyval(P,t); 

plot(t,rl) 

title('Navigation  Filter  Error  Versus  Submerged  Time  For  Hdop  Less  Than  Two') 
xlabel('Time  in  Seconds') 
ylabel('Distance  in  Meters') 
grid 

gtext('error=2.72%  distance  travel') 
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APPENDIX  D.  MATLAB  CODE  FOR  THE  NEW-CTE-BOX 

PATTERN 


whitebg('w'); 

%  State  =  [v  r  psi] 
clear 

%load  in  experrimental  data 

load  d081 101_02.d;d=d081 101_02;clear  d081 101_02; 
Xd  =d(:,10); 

Yd  =d(:,ll); 

psid  =d(:,16); 

drd  =  d(:,37); 

ud  =d(:,17); 

vd  =d(:,18); 

rd  =d(:,23); 

Second  =  d(:,8); 


TRUE  =  1; 

FALSE  =  0; 

DegRad  =  pi/180; 

RadDeg  =  180/pi; 

%State  Model  PArameters 
W  =600.0; 

U=  1.4*3.28; 
g  =  32.174; 

Boy  =  500.0; 
xg  =0.125/12.0; 
m  =  W/g; 

rho  =  1.9903; 

L=  10; 

Iz  =  (l/12)*m*(1.33^2  +  10^2);  %  Approx.  Using  I  =  l/12*m*(a^2  +  b^2) 
Iz  =  Iz*5.0; 

Yv_dot  =  -0.03430*(rbo/2)*L^3; 

Yr_dot  =  -0.00178*(rbo/2)*LM; 

Yv  =  -0.10700*(rbo/2)*L^2; 
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Yr  =  0.01187*(rho/2)*L^3; 

Ydrs  =  (0.01241*(rho/2)*L^2)/2.0;  %  Since  Bow  &  Stem  Lower  Rudders  Removed 
Ydrb  =  (0.01241*(rho/2)*L^2)/2.0; 

Nv_dot  =  -0.00178*(rho/2)*LM; 

%Nr_dot  =  -0.00047*(rho/2)*L^5; 

Nr_dot  =  -Iz; 

Nv  =  -0.00769*(rho/2)*L^3; 

Nr  =  -0.00390*(rho/2)*LM; 

%Ndrs  =  -2.6496/2.0;  %  Since  Bow  &  Stem  Lower  Rudders  Removed 
%Ndrb  =  1.989/2.0; 

%  Below  Modified  on  7/12/00  The  3.5  and  3.4167  is  the  Moment  Arm  Length  in  Feet 

Ndrs  =  -0.01241  *(rho/2)*(L^2)*(3.5)/2.0;  %  Since  Stem  Lower  Rudder  Removed 
Ndrb  =  0.01241  *(rho/2)*(L^2)*(3.4167)/2.0;  %  Since  Bow  Lower  Rudder  Removed 

%  Combining  Stem  &  Bow  Rudder  Effectivness 

Ndr  =  Ndrs  -  Ndrb; 

Ydr  =  Ydrs  -  Ydrb;  %  Cancel  Out 

ml  =  m  -  Yv  dot; 
m2  =  m*xg  -  Yr_dot; 
m3  =  m*xg  -  Nv_dot; 
m4  =  Iz  -  Nr_dot; 

Y1  =Yv; 

Y2  =  Yr; 

Y3  =  U^2*Ydr; 

N1  =Nv; 

N2  =  Nr; 

N3  =  U^2*Ndr; 

A=  [Y1*U  Y2*U;N1*UN2*U]; 

B  =  [Y3  N3]'; 

M  =  [ml  m2;m3  m4]; 

A1  =  inv(M)*A; 

B1  =  inv(M)*B; 

A=[A1(1,1)A1(1,2)  0; 

Al(2,l)  Al(2,2)  0; 

0  10]; 
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B  =  [B1;0]; 
dt  =  0.125; 


t  =  [0:dt:3000]'; 
size(t) 

%  set  initial  conditions 
start=10; 

v(l)  =  vd(start)*3.28; 
r(l)  =  rd(start)/180*pi; 
rRM(l)  =  r(l); 
psi(l)  =  psid(start)./180*pi; 

%  This  is  the  Initial  Position  of  the  Vehicle 
X(l)  =  Xd( start);  %  Meters 
Y(l)  =  Yd(start); 

lcte(l)  =  0.0; 


%  Convert  to  Feet 

%X  =  (2/3)*Y  +  3.333333; 

%this  data  from  track.out  fde 
No_tracks=24; 

Track=[200.0  0.0  2.75  2.75  0  1.25  2.00  0  25.00  8.00  40.00 


200.0 

10.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

190.0 

10.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

190.0 

0.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

200.0 

0.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

200.0 

10.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

190.0 

10.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

190.0 

0.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

200.0 

0.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

200.0 

10.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

190.0 

10.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

190.0 

0.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

200.0 

0.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

200.0 

10.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

190.0 

10.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

190.0 

0.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

200.0 

0.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

200.0 

10.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

190.0 

10.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 

190.0 

0.0 

2.75  2.75 

0 

1.25 

2.00  0  25.00  2.00  40.00 
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200.0  0.0  2.75  2.75  0  1.25  2.00  0  25.00  2.00  40.00 
200.0  10.0  2.75  2.75  0  1.25  2.00  0  25.00  2.00  40.00 
190.0  10.0  2.75  2.75  0  1.25  2.00  0  25.00  2.00  40.00 
190.0  0.0  2.75  2.75  0  1.25  2.00  0  25.00  2.00  40.00 


]; 

track=Track(:,  1 :2); 


%  readin  wayopoints  from  track  data  assumes  track  is  loaded 
for  j=l  :No_tracks, 

X_Way_c(j)  =track(j,l); 

Y_Way_c(j)  =  track(j,2);  end; 


%Set  start  position 

PrevX_Way_c(l)  =  Xd(start); 
PrevY_Way_c(l)  =  Yd(start); 

r_com  =  0.0; 

W_R  =  4.0;loiter=0; 

a  =  Al(2,2); 
b  =  Bl(2); 


a  =  -.3; 
b  =  (9/24)*a; 


x(:,l)  =  [v(l);r(l);psi(l)]; 
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%  Below  are  in  British  Units  for  CTE  Stable  Poly 
Laml  =  2.0; 

Lani2  =  2.0; 

Lam3  =  0.5; 

Lani4  =  0.2; 

%  Below  are  in  British  Units  for  CTE  Sliding  Mode 
%Laml  =  0.75; 

%Lam2  =  0.5; 

Eaml  =  2.0; 

Lam2  =  1.0; 


EtaFlightHeading  =  1 .0; 

PhiFlightHeading  =  0.5; 

%  Below  for  tanh 
Eta_CTE  =  0.1; 

Eta_CTE_Min=  1.0; 

PhiCTE  =  0.5; 

Uc  =  []; 

Vc  =  [] 

INT  =  0; 

PLOTPART  =  0; 

SegLen(l)  =  sqrt((X_Way_c(l)-PrevX_Way_c(l))^2+(Y_Way_c(l) 

PrevY_Way_c(l))^2); 

psi_track(  1 )  =  atan2(Y_Way_c(  1  )-PrevY_Way_c(  1  ),X_Way_c(  1  )-PrevX_Way_c(  1 )); 
for  j=2  :No_tracks, 

SegLen(j)  =  sqrt((X_Way_c(j)-X_Way_c(j-l))^2+(Y_Way_c(j) 

Y_Way_c(j-l))^2); 

psi_track(j)  =  atan2(Y_Way_c(j)-Y_Way_c(j-l),X_Way_c(j)-X_Way_c(j-l)); 
end; 
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SurfPhase  =0.0*ones(l,No_tracks); 

j=i; 

Sigma  =  []; 

Depth_com  =  []; 
dr=[]; 

drl  =  zeros(l,length(t)); 

Depth_com(l)  =  5.0; 

WayPointVertDist_com  =  5*ones(l,No_tracks); 


SURFACETIMERACTIVE  =  FAESE; 

SurfaceTime  =  30.0; 

for  i=l:length(t)-l, 

%for  i=l:20, 

Depth_com(i)  =  WayPointVertDist_com(j); 

X_Way_Error(i)  =  X_Way_c(j)  -  X(i); 

Y_Way_Error(i)  =  Y_Way_c(j)  -  Y(i); 

%  DeWrap  psi  to  within  +/-  2.0*pi; 
psi_cont(i)  =  psi(i); 

while(abs(psi_cont(i))  >  2.0*pi) 
psi_cont(i)  =  psi_cont(i)  -  sign(psi_cont(i))*2.0*pi; 
end; 

psi_errorCTE(i)  =  psi_cont(i)  -  psi_track(j); 

%  DeWrap  psi_error  to  within  +/-  pi; 
while(abs(psi  errorCTE(i))  >  pi) 

psi_errorCTE(i)  =  psi_errorCTE(i)  -  sign(psi_errorCTE(i))*2.0*pi; 
end; 


%  **  Always  Caleulate  this 
Beta  =  v(i)/U; 

Beta  =  0.0; 
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cpsi_e  =  cos(psi_errorCTE(i)+Beta); 
spsi_e  =  sin(psi_errorCTE(i)+Beta); 


s(i)  =  [X_Way_Error(i),Y_Way_Error(i)]*... 

[(X_Way_c(j)-PrevX_Way_c(j)),(Y_Way_c(j)-PrevY_Way_c(j))]'; 

%  s  is  distance  to  go  projected  to  track  line(goes  from  0-100%L) 

s(i)  =  s(i)/SegLen(j); 

Ratio=(  1 . 0-s(i)/SegLen(j ))  *  1 00 . 0 ; 

%** 

%  radial  distance  to  go  to  next  WP 

ss(i)  =  sqrt(X_Way_Error(i)^2  +  Y_Way_Error(i)^2); 

%  dp  is  angle  between  line  of  sight  and  current  track  line 
dp(i)  = ... 

atan2(  (Y_Way_c(j)-PrevY_Way_c(j)),(X_Way_c(j)-PrevX_Way_c(j)) )... 
-  atan2(  Y_Way_Error(i),X_Way_Error(i) ); 

if(dp(i)  >  pi), 
dp(i)  =  dp(i)  -  2.0*pi; 
end; 

cte(i)  =  s(i)*sin(dp(i)); 


if(  abs(psi_errorCTE(i))  >=  40.0*pi/180.0)  |  (s(i)  <  0.0  |((loiter==l)&  s(i)<20) ) 
%  Use  LOS  Control 
LOS(i)=l; 

psi_comLOS  =  atan2(Y_Way_Error(i),X_Way_Error(i)); 

psi_errorLOS(i)  =  psi  comLOS  -  psi_cont(i); 

if(abs(psi_errorLOS(i))  >  pi), 
psi_errorLOS(i)  = ... 

psi_errorLOS(i)  -  2.0*pi*psi_errorLOS(i)/abs(psi_errorLOS(i)); 
end; 

Sigma_FlightHeading  =  0.9499*(r_com  -  r(i))  +  0.1701*psi_errorLOS(i); 
dr(i)  =  -1.5435*(  2.5394*r(i) ... 

+  Eta_FlightHeading*tanh(Sigma_FlightHeading/Phi_FlightHeading)); 


else 
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%  Use  CTE  Controller 

LOS(i)=0;  if(cpsi_e  ~=  0.0),  %  Trap  Div.  by  Zero  ! 

%  STABLE  POLY  Soln 

%  dr(i)  =  (1.0/(U*b*cpsi_e))*(-U*a*r(i)*cpsi_e  +  U*r(i)^2*spsi_e  ... 

%  -  Laml*U*r(i)*cpsi_e  -  Lam2*U*spsi_e  -  3.28*Lam3*cte(i) ... 

%  -  Lam4*Icte); 

%  dr(i)  =  (1.0/(U*b*cpsi_e))*(-U*a*rRM(i)*cpsi_e  +  U*rRM(i)^2*spsi_e  ... 

%  -  Laml*U*rRM(i)*cpsi_e  -  Lam2*U*spsi_e  -  3.28*Lam3*cte(i) ... 

%  -  Lam4*Icte); 

%  SMC  Soln 

%  Eta_CTE  =  Eta_CTE_Min  +  0.5*abs(cte(i)); 

Sigma(i)  =  U*rRM(i)*cpsi_e  +  Laml*U*spsi_e  +  3.28*Lam2*cte(i); 

dr(i)  =  (1.0/(U*b*cpsi_e))*(-U*a*rRM(i)*cpsi_e  +  U*rRM(i)^2*spsi_e  ... 

-  Laml*U*rRM(i)*cpsi_e  -  Lam2*U*spsi_e  -  Eta_CTE*(Sigma(i)/Phi_CTE)); 

%  dr(i)  =  (1.0/(U*b*cpsi_e))*(-U*a*rRM(i)*cpsi_e  +  U*rRM(i)^2*spsi_e  ... 

%  -  Laml*U*rRM(i)*cpsi_e  -  Lam2*U*spsi_e)  ... 

%  -  Eta_CTE/sign(U*b*cpsi_e)*Sigma(i)/Phi_CTE; 

%  -  Eta_CTE/sign(U*b*cpsi_e)*tanh(Sigma(i)/Phi_CTE); 

else 


dr(i)  =  dr(i-l); 
end; 

%  Int  of  CTE  in  meters-sec 

if(INT==l), 

Icte  =  Icte  +  dt*cte(i); 

else 


Icte  =  0.0; 

end; 

UseVector(i,:)  =  [1  t(i)]; 


end;  %  End  of  CTE  Controller 


%  use  LOS  if  near  to  loiter  point 
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%  if  (loiter==l)&  s(i)<10;  dr(i)=drlos(i);end; 


%  Surface  Phase  Logic  (Independent  of  LOS  or  CTE) 

if(SurfPhaseG)  ==  TRUE) 
if(SURFACE_TIMER_ACTIVE  ==  FALSE) 
if(Ratio  >40.0) 

%  Start  a  Timer 

SURFACETIMERACTIVE  =  TRUE; 

Depth_com(i)  =  0.0; 

SurfaceWait  =  SurfaceTime  +  t(i); 

SurfaceWait 

end; 

end; 

end; 

if(SURFACE_TIMER_ACTIVE  ==  TRUE) 
if(t(i)  >=  SurfaceWait) 

SURFACETIMERACTIVE  =  FALSE; 

Depth  com(i)  =  WayPointVertDist  com(i); 

SurfPhase(j)  =  0; 
else 

Depth_com(i)  =  0.0; 
end; 
end; 


%if(i==800) 

%  X(I,i)  =  20.0; 
%  end; 


%if(i==2500) 

%  X(I,i)  =  60.0; 
%end; 


if(abs(dr(i))  >  0.4) 

dr(i)  =  0.4*sign(dr(i)); 

end; 


%dr(i)  =  22.5*pi/180; 


%  Model  drl  is  the  actual  lagged  rudder,  dr  is  the  rudder  command. 


75 


taudr=0.255; 

drl(i+ 1  )=drl(i)+dt*(dr(i)-drl(i))/taudr; 

%  if(abs(drl(i))  >  0.4) 

%  drl(i)  =  0.4*sign(drl(i)); 

%  end; 


%Jay  Johnson  Model; 

Yv  = -68.16; 

Yr  =  406.3; 

Ydr  =  70.0; 

Nv  = -10.89; 

Nr  =  -88.34; 

Ndr  = -35.47; 

MY  =  456.76; 

IN  =  215; 

M=diag([MY,IN,l]); 

AA=[Yv,Yr,0;Nv,Nr,0;0,l,0];BB=[Ydr;Ndr;0]; 

A=inv(M)*AA;B=inv(M)*BB; 

x_dot(:,i+l)  =  [  A(l,l)*v(i)  +  A(l,2)*r(i)  +  B(l)*drl(i); 

A(2,l)*v(i)  +  A(2,2)*r(i)  -I-  B(2)*drl(i); 

r(i)]; 

x(:,i+l)=x(:,i)+dt*x_dot(:,i); 
v(i-i-l)  =  x(l,i+l)/12; 
r(i-M)  =x(2,i+l); 
psi(i+l)  =  x(3,i+l); 
rRM(i+l)=r(i+l); 

%  Throw  in  some  Waves 
%Uc(i)  =  -0.5*sin(2*pi*t(i)/5); 

%Vc(i)  =  0.5*sin(2*pi*t(i)/5); 

%Model  using  system  ID  results  from  Bay  tests 

%  rRM(i-l-l)  =  rRM(i)  -I-  dt*(a*rRM(i)  -I-  b*drl(i)); 

%  psi(i+l)  =  psi(i)  +  dt*rRM(i); 

%  side  slip  added  proprtional  to  turn  rate  from  AZORES  data  V  in  ft/sec 
%  v(i+l)  =  1.0*rRM(i+l)*3.28; 

Uc  =  -0.15;%*0.0;Northemly  current 
Vc  =  0.15;%*0.0;Westerly  Current 
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%Kinematics 

X(i+1)  =  X(i)  +  (Uc  +  (U/3.28)*cos(psi(i))  -  v(i)/3.28*sin(psi(i))  )*dt; 
Y(i+1)  =  Y(i)  +  (Vc  +  (U/3.28)*sin(psi(i))  +  v(i)/3.28*cos(psi(i))  )*dt; 


%  Check  to  See  if  we  are  Within  the  Watch  Radius 


if(sqrt(X_Way_Error(i)^2.0  +  Y_Way_Error(i)^2.0)  <=  W_R  |  s(i)  <  0.0), 

if  (loiter~=l);  %insert  this  for  loiter  node 
INT=  1; 

if(j==No_tracks), 

PLOTPART  =  1; 

break; 

end; 

PrevX_Way_cG+l)  =  X_Way_c(j); 

PrevY_Way_c(j+l)  =  Y_Way_c(j); 

j=j+i; 

end; 

end; 

end; 

%end  update  loop 
%update 
dr(i+l)  =  dr(i); 
cte(i+l)  =  cte(i); 
s(i+l)  =  s(i); 
ss(i+l)  =  ss(i); 

if(PLOT_PART), 

figure(l);clf, 

plot(t([l:i+l]),psi*l  80/pi, t([l:length(psid)]),psid,'g.'); 
hold; 

plot(t([  1  :i+ 1  ]),dr*  1 80/pi, 'r',t([  1  :length(psid)]),drd,'g.');grid; 

TlTLE('psi  in  basic  color,  dr  in  red'); 
hold;zooni  on; 

figure(2);clf, 

plot(t([  1  :i+ 1  ]),cte,'b',t(  1  :length(LOS)),LOS,'y'); 
hold; 

plot(t([l:length(s)]),s,'r');TITLE('cte  in  blue,LOS  in  yellow,  s  in  red') 


77 


hold;zoom  on;grid 


else 

figure(l);clf, 
plot(t,psi*  180/pi); 
hold; 

plot(t,drl*  1 80/pi, 'r'); 

hold;grid; 

figure(2); 

plot(t,cte,'b',t(l:length(LOS)),LOS,y); 

hold; 

plot(t,s,'r'); 

plot(t,ss,'g');grid; 

TITLE('s  in  red,ssin  green,  cte  in  blue') 
hold;zooni  on; 

end; 


figure(3);clf, 

plot(Yd,Xd,'b.',Y,X,'y');grid; 

hold 

plot([Y_Way_c(  1 )  PrevY_Way_c(  1 )], [X_Way_c(  1 )  PrevX_Way_c(  1  )],'r'); 

plot(Y_Way_c,X_Way_c,'r'); 

plot(Y_Way_c,X_Way_c,'m*'), 

hold;zooni  on; 

figure(4),  elf, 

plot(t([  1  :i+ 1  ]),r*  1 80/pi,'r',t([  1  :length(psid)]),rd,'g.');grid; 
figure(5),clf,plot(t([l  :i+l]),vd(l  :i+l),'r',t([l  :i+l]),v,'g.') 
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